

#include "HeaderFiles.h"


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void lsm6dsr_init(void)
{
	
	iic_byte_write(LSM6DSR_I3C_BUS_AVB, LSM6DSR_I3C_DISABLE & 0X03);   			
	iic_byte_write(LSM6DSR_CTRL3_C, 0X40);
	iic_byte_write(LSM6DSR_CTRL1_XL, ACC_ODR_208HZ | LSM6DSR_8g | ACC_USE_LPF1);	
	iic_byte_write(LSM6DSR_CTRL2_G, GYRO_ODR_208HZ | LSM6DSR_2000dps);				
	iic_byte_write(LSM6DSR_CTRL7_G, G_HM_MODE_ENABLE);								
	iic_byte_write(LSM6DSR_CTRL6_C, XL_HM_MODE_ENABLE);							

	delay_1ms(100);

}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void get_lsm6dsr_gyro_z(float32_t *ang_spd)
{
	
	uint8_t data_H = 0;
	uint8_t data_L = 0;
	int16_t gyro_rslt = 0;

	iic_buffer_read(&data_L, LSM6DSR_OUTZ_L_GYRO, 1);
	iic_buffer_read(&data_H, LSM6DSR_OUTZ_H_GYRO, 1);			

	gyro_rslt = data_L + data_H * 256;
	
	*ang_spd = 2000.0f * (float32_t)gyro_rslt / 32767.0f;
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void get_lsm6dsr_gyro_xyz(float32_t *gyro)
{
	uint8_t data_H = 0;
	uint8_t data_L = 0;
	int16_t gyro_rslt = 0;
	uint8_t addr_start = LSM6DSR_OUTX_L_GYRO;
	
	for(uint8_t i=0;i<3;i++)
	{
		iic_buffer_read(&data_L, addr_start, 1);
		addr_start++;
		iic_buffer_read(&data_H, addr_start, 1);
		addr_start++;
		
		gyro_rslt = data_L + data_H * 256;
		
		*(gyro + i) = 2000.0f * (float32_t)gyro_rslt / 32768.0f;
	}
}

/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void get_lsm6dsr_accel_xyz(float32_t *accel)
{
	uint8_t data_H = 0;
	uint8_t data_L = 0;
	int16_t accel_rslt = 0;
	uint8_t addr_start = LSM6DSR_OUTX_L_ACC;
	
	for(uint8_t i=0;i<3;i++)
	{
		iic_buffer_read(&data_L, addr_start, 1);
		addr_start++;
		iic_buffer_read(&data_H, addr_start, 1);
		addr_start++;
		
		accel_rslt = data_L + data_H * 256;
		
		*(accel + i) = 8.0f * (float32_t)accel_rslt / 32768.0f;
	}


}
	


